import numpy as np
# from rich import print
from scipy.optimize import minimize

from tma.utils import settings as s

initial_guess = [5000.0, 180.0, 5.0]


def cal_track_metric(params, own_positions, bearings, time_steps):
    L1_distance, crs, spd = params
    crs_rad = np.radians(crs)
    brg = np.radians(bearings)

    # target speed components
    u = spd * np.sin(crs_rad)
    v = spd * np.cos(crs_rad)

    # target position at first bearing
    m0, n0 = own_positions[0]
    a = m0 + L1_distance * np.sin(brg[0])
    b = n0 + L1_distance * np.cos(brg[0])

    # sum of errors
    f_sum = 0
    for idx in range(len(time_steps)):
        m, n = own_positions[idx]
        x = a + u * time_steps[idx]
        y = b + v * time_steps[idx]
        f_t = (y - n) * np.sin(brg[idx]) - (x - m) * np.cos(brg[idx])
        f_sum += f_t**2
    return f_sum


def predict(own_pos: list[tuple[float, float]], bearings: list[float], time_steps: list[int]):
    own_pos = list([float(i[0]), float(i[1])] for i in own_pos)
    # bearings = list(float(i) for i in bearings)
    # time_steps = list(float(i) for i in time_steps)
    ts = list(i - time_steps[0] for i in time_steps)
    res = minimize(cal_track_metric, initial_guess, args=(own_pos, bearings, ts), method="BFGS")
    error = cal_track_metric(res.x, own_pos, bearings, ts)

    L1_distance, crs, spd = res.x
    return res.success, res.status, res.nit, res.message, error, L1_distance, spd, crs
